#include "communication/communication_plugin.h"
#include <pluginlib/class_list_macros.h>
#include <smarteye_common/DesiredAttitude.h>
#include <smarteye_common/controlMsg.h>


namespace commplugin {

typedef struct
{
    float h_sp;             //导航高度
    float va_sp; 	   //导航速度，如果超出安全门限，则以上下限飞行
    float yaw_sp;
    float roll_sp;
    float pitch_sp;
} StateSP;


class State_SP_Plugin : public CommPlugin {
public:
        State_SP_Plugin() :
                state_sp_nh("~"),
                station_link(nullptr)
        { }

        void initialize(augmavconn::AugMavConnInterface::AugPtr station_link_, int target_system_id_, int target_component_id_)
        {

            station_link = station_link_;
            target_system_id = target_system_id_;
            target_component_id = target_component_id_;
            guideSub = state_sp_nh.subscribe("/formation/desireAttitude",10, &State_SP_Plugin::receiveGuide,this);

            rpyt_sub = state_sp_nh.subscribe("/control/flight_state/rpyt",10, &State_SP_Plugin::receiveRpyt,this);

            stateSPTimer = state_sp_nh.createTimer(ros::Duration(1),&State_SP_Plugin::timer_cb,this);
        }

        const message_map get_rx_handlers() {
                return { /* Rx disabled */ };
        }

private:
        ros::NodeHandle state_sp_nh;
        augmavconn::AugMavConnInterface::AugPtr station_link;
        int target_system_id;
        int target_component_id;
        StateSP stateSP;
        ros::Subscriber guideSub;
        ros::Subscriber rpyt_sub;
        ros::Timer stateSPTimer;
        bool IsRecvGuide;
        bool IsRecvRpyt;
        std::recursive_mutex mutex;

        //need a lock

        void receiveRpyt(smarteye_common::controlMsg rpytmsg)
        {
            //ROS_INFO("guide %f,\t %f",rpytmsg.roll,rpytmsg.pitch);
            std::unique_lock<std::recursive_mutex> lock(mutex);
            IsRecvRpyt = true;
            stateSP.roll_sp = rpytmsg.roll;
            stateSP.pitch_sp = rpytmsg.pitch;
            //stateSP.yaw_sp = rpytmsg.yaw;
            lock.unlock();
        }

        void receiveGuide(smarteye_common::DesiredAttitude guidemsg)
        {
            //ROS_INFO("rpyt %f,\t %f",guidemsg.Gd_Height,guidemsg.Gd_V);
            std::unique_lock<std::recursive_mutex> lock(mutex);
            IsRecvGuide = true;
            stateSP.yaw_sp = guidemsg.Gd_Ang;
            stateSP.h_sp = guidemsg.Gd_Height;
            stateSP.va_sp = guidemsg.Gd_V;
            lock.unlock();
        }

        void timer_cb(const ros::TimerEvent &event)
        {
            if (IsRecvGuide && IsRecvRpyt)
            {
                //ROS_INFO("%f,%f,%f,%f,%f",stateSP.va_sp,stateSP.h_sp,stateSP.yaw_sp,stateSP.roll_sp,stateSP.pitch_sp);
                send_state_sp();
            }
            std::unique_lock<std::recursive_mutex> lock(mutex);
            IsRecvGuide = false;
            IsRecvRpyt = false;
            lock.unlock();
        }

        void send_state_sp(void)
        {
            mavlink_message_t msg;
            mavlink_msg_state_sp_pack_chan(PACK_CHAN(station_link),
                                           &msg,0,
                                           stateSP.va_sp*1E3,
                                           stateSP.h_sp*1E3,
                                           stateSP.yaw_sp*1E3,
                                           stateSP.roll_sp*1E3,
                                           stateSP.pitch_sp*1E3);
            station_link->send_message(&msg);
        }

};


};	// namespace commplugin

PLUGINLIB_EXPORT_CLASS(commplugin::State_SP_Plugin, commplugin::CommPlugin)
